//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty.  In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
//    claim that you wrote the original software. If you use this software
//    in a product, an acknowledgment in the product documentation would be
//    appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
//    misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//

#define _USE_MATH_DEFINES
#include <math.h>
#include <stdio.h>
#include <string.h>
#include "SDL.h"
#include "SDL_opengl.h"
#include "imgui.h"
#include "InputGeom.h"
#include "Sample.h"
#include "Sample_TileMesh.h"
#include "Recast.h"
#include "RecastDebugDraw.h"
#include "DetourNavMesh.h"
#include "DetourNavMeshBuilder.h"
#include "DetourDebugDraw.h"
#include "NavMeshTesterTool.h"
#include "OffMeshConnectionTool.h"
#include "ConvexVolumeTool.h"
#include "CrowdTool.h"

#ifdef WIN32
#    define snprintf _snprintf
#endif


inline unsigned int nextPow2(unsigned int v)
{
    v--;
    v |= v >> 1;
    v |= v >> 2;
    v |= v >> 4;
    v |= v >> 8;
    v |= v >> 16;
    v++;
    return v;
}

inline unsigned int ilog2(unsigned int v)
{
    unsigned int r;
    unsigned int shift;
    r = (v > 0xffff) << 4; v >>= r;
    shift = (v > 0xff) << 3; v >>= shift; r |= shift;
    shift = (v > 0xf) << 2; v >>= shift; r |= shift;
    shift = (v > 0x3) << 1; v >>= shift; r |= shift;
    r |= (v >> 1);
    return r;
}

class NavMeshTileTool : public SampleTool
{
    Sample_TileMesh* m_sample;
    float m_hitPos[3];
    bool m_hitPosSet;
    float m_agentRadius;
    
public:

    NavMeshTileTool() :
        m_sample(0),
        m_hitPosSet(false),
        m_agentRadius(0)
    {
        m_hitPos[0] = m_hitPos[1] = m_hitPos[2] = 0;
    }

    virtual ~NavMeshTileTool()
    {
    }

    virtual int type() { return TOOL_TILE_EDIT; }

    virtual void init(Sample* sample)
    {
        m_sample = (Sample_TileMesh*)sample; 
    }
    
    virtual void reset() {}

    virtual void handleMenu()
    {
        imguiLabel("Create Tiles");
        if (imguiButton("Create All"))
        {
            if (m_sample)
                m_sample->buildAllTiles();
        }
        if (imguiButton("Remove All"))
        {
            if (m_sample)
                m_sample->removeAllTiles();
        }
        imguiValue("Click LMB to create a tile.");
        imguiValue("Shift+LMB to remove a tile.");
    }

    virtual void handleClick(const float* /*s*/, const float* p, bool shift)
    {
        m_hitPosSet = true;
        rcVcopy(m_hitPos,p);
        if (m_sample)
        {
            if (shift)
                m_sample->removeTile(m_hitPos);
            else
                m_sample->buildTile(m_hitPos);
        }
    }

    virtual void handleToggle() {}

    virtual void handleStep() {}

    virtual void handleUpdate(const float /*dt*/) {}
    
    virtual void handleRender()
    {
        if (m_hitPosSet)
        {
            const float s = m_sample->getAgentRadius();
            glColor4ub(0,0,0,128);
            glLineWidth(2.0f);
            glBegin(GL_LINES);
            glVertex3f(m_hitPos[0]-s,m_hitPos[1]+0.1f,m_hitPos[2]);
            glVertex3f(m_hitPos[0]+s,m_hitPos[1]+0.1f,m_hitPos[2]);
            glVertex3f(m_hitPos[0],m_hitPos[1]-s+0.1f,m_hitPos[2]);
            glVertex3f(m_hitPos[0],m_hitPos[1]+s+0.1f,m_hitPos[2]);
            glVertex3f(m_hitPos[0],m_hitPos[1]+0.1f,m_hitPos[2]-s);
            glVertex3f(m_hitPos[0],m_hitPos[1]+0.1f,m_hitPos[2]+s);
            glEnd();
            glLineWidth(1.0f);
        }
    }
    
    virtual void handleRenderOverlay(double* proj, double* model, int* view)
    {
        GLdouble x, y, z;
        if (m_hitPosSet && gluProject((GLdouble)m_hitPos[0], (GLdouble)m_hitPos[1], (GLdouble)m_hitPos[2],
                                      model, proj, view, &x, &y, &z))
        {
            int tx=0, ty=0;
            m_sample->getTilePos(m_hitPos, tx, ty);
            char text[32];
            snprintf(text,32,"(%d,%d)", tx,ty);
            imguiDrawText((int)x, (int)y-25, IMGUI_ALIGN_CENTER, text, imguiRGBA(0,0,0,220));
        }
        
    }
};




Sample_TileMesh::Sample_TileMesh() :
    m_keepInterResults(false),
    m_buildAll(true),
    m_totalBuildTimeMs(0),
    m_drawPortals(false),
    m_triareas(0),
    m_solid(0),
    m_chf(0),
    m_cset(0),
    m_pmesh(0),
    m_dmesh(0),
    m_drawMode(DRAWMODE_NAVMESH),
    m_maxTiles(0),
    m_maxPolysPerTile(0),
    m_tileSize(32),
    m_tileCol(duRGBA(0,0,0,32)),
    m_tileBuildTime(0),
    m_tileMemUsage(0),
    m_tileTriCount(0)
{
    resetCommonSettings();
    memset(m_tileBmin, 0, sizeof(m_tileBmin));
    memset(m_tileBmax, 0, sizeof(m_tileBmax));
    
    setTool(new NavMeshTileTool);
}

Sample_TileMesh::~Sample_TileMesh()
{
    cleanup();
    dtFreeNavMesh(m_navMesh);
    m_navMesh = 0;
}

void Sample_TileMesh::cleanup()
{
    delete [] m_triareas;
    m_triareas = 0;
    rcFreeHeightField(m_solid);
    m_solid = 0;
    rcFreeCompactHeightfield(m_chf);
    m_chf = 0;
    rcFreeContourSet(m_cset);
    m_cset = 0;
    rcFreePolyMesh(m_pmesh);
    m_pmesh = 0;
    rcFreePolyMeshDetail(m_dmesh);
    m_dmesh = 0;
}


static const int NAVMESHSET_MAGIC = 'M'<<24 | 'S'<<16 | 'E'<<8 | 'T'; //'MSET';
static const int NAVMESHSET_VERSION = 1;

struct NavMeshSetHeader
{
    int magic;
    int version;
    int numTiles;
    dtNavMeshParams params;
};

struct NavMeshTileHeader
{
    dtTileRef tileRef;
    int dataSize;
};

void Sample_TileMesh::saveAll(const char* path, const dtNavMesh* mesh)
{
    if (!mesh) return;
    
    FILE* fp = fopen(path, "wb");
    if (!fp)
        return;
    
    // Store header.
    NavMeshSetHeader header;
    header.magic = NAVMESHSET_MAGIC;
    header.version = NAVMESHSET_VERSION;
    header.numTiles = 0;
    for (int i = 0; i < mesh->getMaxTiles(); ++i)
    {
        const dtMeshTile* tile = mesh->getTile(i);
        if (!tile || !tile->header || !tile->dataSize) continue;
        header.numTiles++;
    }
    memcpy(&header.params, mesh->getParams(), sizeof(dtNavMeshParams));
    fwrite(&header, sizeof(NavMeshSetHeader), 1, fp);

    // Store tiles.
    for (int i = 0; i < mesh->getMaxTiles(); ++i)
    {
        const dtMeshTile* tile = mesh->getTile(i);
        if (!tile || !tile->header || !tile->dataSize) continue;

        NavMeshTileHeader tileHeader;
        tileHeader.tileRef = mesh->getTileRef(tile);
        tileHeader.dataSize = tile->dataSize;
        fwrite(&tileHeader, sizeof(tileHeader), 1, fp);

        fwrite(tile->data, tile->dataSize, 1, fp);
    }

    fclose(fp);
}

dtNavMesh* Sample_TileMesh::loadAll(const char* path)
{
    FILE* fp = fopen(path, "rb");
    if (!fp) return 0;
    
    // Read header.
    NavMeshSetHeader header;
    fread(&header, sizeof(NavMeshSetHeader), 1, fp);
    if (header.magic != NAVMESHSET_MAGIC)
    {
        fclose(fp);
        return 0;
    }
    if (header.version != NAVMESHSET_VERSION)
    {
        fclose(fp);
        return 0;
    }
    
    dtNavMesh* mesh = dtAllocNavMesh();
    if (!mesh)
    {
        fclose(fp);
        return 0;
    }
    dtStatus status = mesh->init(&header.params);
    if (dtStatusFailed(status))
    {
        fclose(fp);
        return 0;
    }
        
    // Read tiles.
    for (int i = 0; i < header.numTiles; ++i)
    {
        NavMeshTileHeader tileHeader;
        fread(&tileHeader, sizeof(tileHeader), 1, fp);
        if (!tileHeader.tileRef || !tileHeader.dataSize)
            break;

        unsigned char* data = (unsigned char*)dtAlloc(tileHeader.dataSize, DT_ALLOC_PERM);
        if (!data) break;
        memset(data, 0, tileHeader.dataSize);
        fread(data, tileHeader.dataSize, 1, fp);
        
        mesh->addTile(data, tileHeader.dataSize, DT_TILE_FREE_DATA, tileHeader.tileRef, 0);
    }
    
    fclose(fp);
    
    return mesh;
}

void Sample_TileMesh::handleSettings()
{
    Sample::handleCommonSettings();

    if (imguiCheck("Keep Itermediate Results", m_keepInterResults))
        m_keepInterResults = !m_keepInterResults;

    if (imguiCheck("Build All Tiles", m_buildAll))
        m_buildAll = !m_buildAll;
    
    imguiLabel("Tiling");
    imguiSlider("TileSize", &m_tileSize, 16.0f, 1024.0f, 16.0f);
    
    if (m_geom)
    {
        const float* bmin = m_geom->getMeshBoundsMin();
        const float* bmax = m_geom->getMeshBoundsMax();
        char text[64];
        int gw = 0, gh = 0;
        rcCalcGridSize(bmin, bmax, m_cellSize, &gw, &gh);
        const int ts = (int)m_tileSize;
        const int tw = (gw + ts-1) / ts;
        const int th = (gh + ts-1) / ts;
        snprintf(text, 64, "Tiles  %d x %d", tw, th);
        imguiValue(text);

        // Max tiles and max polys affect how the tile IDs are caculated.
        // There are 22 bits available for identifying a tile and a polygon.
        int tileBits = rcMin((int)ilog2(nextPow2(tw*th)), 14);
        if (tileBits > 14) tileBits = 14;
        int polyBits = 22 - tileBits;
        m_maxTiles = 1 << tileBits;
        m_maxPolysPerTile = 1 << polyBits;
        snprintf(text, 64, "Max Tiles  %d", m_maxTiles);
        imguiValue(text);
        snprintf(text, 64, "Max Polys  %d", m_maxPolysPerTile);
        imguiValue(text);
    }
    else
    {
        m_maxTiles = 0;
        m_maxPolysPerTile = 0;
    }
    
    imguiSeparator();
    
    imguiIndent();
    imguiIndent();
    
    if (imguiButton("Save"))
    {
        saveAll("all_tiles_navmesh.bin", m_navMesh);
    }

    if (imguiButton("Load"))
    {
        dtFreeNavMesh(m_navMesh);
        m_navMesh = loadAll("all_tiles_navmesh.bin");
        m_navQuery->init(m_navMesh, 2048);
    }

    imguiUnindent();
    imguiUnindent();
    
    char msg[64];
    snprintf(msg, 64, "Build Time: %.1fms", m_totalBuildTimeMs);
    imguiLabel(msg);
    
    imguiSeparator();
    
    imguiSeparator();
    
}

void Sample_TileMesh::handleTools()
{
    int type = !m_tool ? TOOL_NONE : m_tool->type();

    if (imguiCheck("Test Navmesh", type == TOOL_NAVMESH_TESTER))
    {
        setTool(new NavMeshTesterTool);
    }
    if (imguiCheck("Create Tiles", type == TOOL_TILE_EDIT))
    {
        setTool(new NavMeshTileTool);
    }
    if (imguiCheck("Create Off-Mesh Links", type == TOOL_OFFMESH_CONNECTION))
    {
        setTool(new OffMeshConnectionTool);
    }
    if (imguiCheck("Create Convex Volumes", type == TOOL_CONVEX_VOLUME))
    {
        setTool(new ConvexVolumeTool);
    }
    if (imguiCheck("Create Crowds", type == TOOL_CROWD))
    {
        setTool(new CrowdTool);
    }
    
    imguiSeparatorLine();

    imguiIndent();

    if (m_tool)
        m_tool->handleMenu();

    imguiUnindent();
}

void Sample_TileMesh::handleDebugMode()
{
    // Check which modes are valid.
    bool valid[MAX_DRAWMODE];
    for (int i = 0; i < MAX_DRAWMODE; ++i)
        valid[i] = false;
    
    if (m_geom)
    {
        valid[DRAWMODE_NAVMESH] = m_navMesh != 0;
        valid[DRAWMODE_NAVMESH_TRANS] = m_navMesh != 0;
        valid[DRAWMODE_NAVMESH_BVTREE] = m_navMesh != 0;
        valid[DRAWMODE_NAVMESH_NODES] = m_navQuery != 0;
        valid[DRAWMODE_NAVMESH_PORTALS] = m_navMesh != 0;
        valid[DRAWMODE_NAVMESH_INVIS] = m_navMesh != 0;
        valid[DRAWMODE_MESH] = true;
        valid[DRAWMODE_VOXELS] = m_solid != 0;
        valid[DRAWMODE_VOXELS_WALKABLE] = m_solid != 0;
        valid[DRAWMODE_COMPACT] = m_chf != 0;
        valid[DRAWMODE_COMPACT_DISTANCE] = m_chf != 0;
        valid[DRAWMODE_COMPACT_REGIONS] = m_chf != 0;
        valid[DRAWMODE_REGION_CONNECTIONS] = m_cset != 0;
        valid[DRAWMODE_RAW_CONTOURS] = m_cset != 0;
        valid[DRAWMODE_BOTH_CONTOURS] = m_cset != 0;
        valid[DRAWMODE_CONTOURS] = m_cset != 0;
        valid[DRAWMODE_POLYMESH] = m_pmesh != 0;
        valid[DRAWMODE_POLYMESH_DETAIL] = m_dmesh != 0;
    }
    
    int unavail = 0;
    for (int i = 0; i < MAX_DRAWMODE; ++i)
        if (!valid[i]) unavail++;
    
    if (unavail == MAX_DRAWMODE)
        return;
    
    imguiLabel("Draw");
    if (imguiCheck("Input Mesh", m_drawMode == DRAWMODE_MESH, valid[DRAWMODE_MESH]))
        m_drawMode = DRAWMODE_MESH;
    if (imguiCheck("Navmesh", m_drawMode == DRAWMODE_NAVMESH, valid[DRAWMODE_NAVMESH]))
        m_drawMode = DRAWMODE_NAVMESH;
    if (imguiCheck("Navmesh Invis", m_drawMode == DRAWMODE_NAVMESH_INVIS, valid[DRAWMODE_NAVMESH_INVIS]))
        m_drawMode = DRAWMODE_NAVMESH_INVIS;
    if (imguiCheck("Navmesh Trans", m_drawMode == DRAWMODE_NAVMESH_TRANS, valid[DRAWMODE_NAVMESH_TRANS]))
        m_drawMode = DRAWMODE_NAVMESH_TRANS;
    if (imguiCheck("Navmesh BVTree", m_drawMode == DRAWMODE_NAVMESH_BVTREE, valid[DRAWMODE_NAVMESH_BVTREE]))
        m_drawMode = DRAWMODE_NAVMESH_BVTREE;
    if (imguiCheck("Navmesh Nodes", m_drawMode == DRAWMODE_NAVMESH_NODES, valid[DRAWMODE_NAVMESH_NODES]))
        m_drawMode = DRAWMODE_NAVMESH_NODES;
    if (imguiCheck("Navmesh Portals", m_drawMode == DRAWMODE_NAVMESH_PORTALS, valid[DRAWMODE_NAVMESH_PORTALS]))
        m_drawMode = DRAWMODE_NAVMESH_PORTALS;
    if (imguiCheck("Voxels", m_drawMode == DRAWMODE_VOXELS, valid[DRAWMODE_VOXELS]))
        m_drawMode = DRAWMODE_VOXELS;
    if (imguiCheck("Walkable Voxels", m_drawMode == DRAWMODE_VOXELS_WALKABLE, valid[DRAWMODE_VOXELS_WALKABLE]))
        m_drawMode = DRAWMODE_VOXELS_WALKABLE;
    if (imguiCheck("Compact", m_drawMode == DRAWMODE_COMPACT, valid[DRAWMODE_COMPACT]))
        m_drawMode = DRAWMODE_COMPACT;
    if (imguiCheck("Compact Distance", m_drawMode == DRAWMODE_COMPACT_DISTANCE, valid[DRAWMODE_COMPACT_DISTANCE]))
        m_drawMode = DRAWMODE_COMPACT_DISTANCE;
    if (imguiCheck("Compact Regions", m_drawMode == DRAWMODE_COMPACT_REGIONS, valid[DRAWMODE_COMPACT_REGIONS]))
        m_drawMode = DRAWMODE_COMPACT_REGIONS;
    if (imguiCheck("Region Connections", m_drawMode == DRAWMODE_REGION_CONNECTIONS, valid[DRAWMODE_REGION_CONNECTIONS]))
        m_drawMode = DRAWMODE_REGION_CONNECTIONS;
    if (imguiCheck("Raw Contours", m_drawMode == DRAWMODE_RAW_CONTOURS, valid[DRAWMODE_RAW_CONTOURS]))
        m_drawMode = DRAWMODE_RAW_CONTOURS;
    if (imguiCheck("Both Contours", m_drawMode == DRAWMODE_BOTH_CONTOURS, valid[DRAWMODE_BOTH_CONTOURS]))
        m_drawMode = DRAWMODE_BOTH_CONTOURS;
    if (imguiCheck("Contours", m_drawMode == DRAWMODE_CONTOURS, valid[DRAWMODE_CONTOURS]))
        m_drawMode = DRAWMODE_CONTOURS;
    if (imguiCheck("Poly Mesh", m_drawMode == DRAWMODE_POLYMESH, valid[DRAWMODE_POLYMESH]))
        m_drawMode = DRAWMODE_POLYMESH;
    if (imguiCheck("Poly Mesh Detail", m_drawMode == DRAWMODE_POLYMESH_DETAIL, valid[DRAWMODE_POLYMESH_DETAIL]))
        m_drawMode = DRAWMODE_POLYMESH_DETAIL;
    
    if (unavail)
    {
        imguiValue("Tick 'Keep Itermediate Results'");
        imguiValue("rebuild some tiles to see");
        imguiValue("more debug mode options.");
    }
}

void Sample_TileMesh::handleRender()
{
    if (!m_geom || !m_geom->getMesh())
        return;
    
    DebugDrawGL dd;

    // Draw mesh
    if (m_drawMode == DRAWMODE_MESH)
    {
        // Draw mesh
        duDebugDrawTriMeshSlope(&dd, m_geom->getMesh()->getVerts(), m_geom->getMesh()->getVertCount(),
                                m_geom->getMesh()->getTris(), m_geom->getMesh()->getNormals(), m_geom->getMesh()->getTriCount(),
                                m_agentMaxSlope);
        m_geom->drawOffMeshConnections(&dd);
    }
    else if (m_drawMode != DRAWMODE_NAVMESH_TRANS)
    {
        // Draw mesh
        duDebugDrawTriMesh(&dd, m_geom->getMesh()->getVerts(), m_geom->getMesh()->getVertCount(),
                           m_geom->getMesh()->getTris(), m_geom->getMesh()->getNormals(), m_geom->getMesh()->getTriCount(), 0);
        m_geom->drawOffMeshConnections(&dd);
    }
    
/*    duDebugDrawTriMesh(&dd, m_geom->getMesh()->getVerts(), m_geom->getMesh()->getVertCount(),
                       m_geom->getMesh()->getTris(), m_geom->getMesh()->getNormals(), m_geom->getMesh()->getTriCount(), 0);
    m_geom->drawOffMeshConnections(&dd);*/
    
    glDepthMask(GL_FALSE);
    
    // Draw bounds
    const float* bmin = m_geom->getMeshBoundsMin();
    const float* bmax = m_geom->getMeshBoundsMax();
    duDebugDrawBoxWire(&dd, bmin[0],bmin[1],bmin[2], bmax[0],bmax[1],bmax[2], duRGBA(255,255,255,128), 1.0f);
    
    // Tiling grid.
    int gw = 0, gh = 0;
    rcCalcGridSize(bmin, bmax, m_cellSize, &gw, &gh);
    const int tw = (gw + (int)m_tileSize-1) / (int)m_tileSize;
    const int th = (gh + (int)m_tileSize-1) / (int)m_tileSize;
    const float s = m_tileSize*m_cellSize;
    duDebugDrawGridXZ(&dd, bmin[0],bmin[1],bmin[2], tw,th, s, duRGBA(0,0,0,64), 1.0f);
    
    // Draw active tile
    duDebugDrawBoxWire(&dd, m_tileBmin[0],m_tileBmin[1],m_tileBmin[2], m_tileBmax[0],m_tileBmax[1],m_tileBmax[2], m_tileCol, 2.0f);
    
/*    if (m_navMesh)
    {
        duDebugDrawNavMeshWithClosedList(&dd, *m_navMesh, *m_navQuery, m_navMeshDrawFlags);
        if (m_drawPortals)
            duDebugDrawNavMeshPortals(&dd, *m_navMesh);
    }*/
    
    if (m_navMesh && m_navQuery &&
        (m_drawMode == DRAWMODE_NAVMESH ||
         m_drawMode == DRAWMODE_NAVMESH_TRANS ||
         m_drawMode == DRAWMODE_NAVMESH_BVTREE ||
         m_drawMode == DRAWMODE_NAVMESH_NODES ||
         m_drawMode == DRAWMODE_NAVMESH_PORTALS ||
         m_drawMode == DRAWMODE_NAVMESH_INVIS))
    {
        if (m_drawMode != DRAWMODE_NAVMESH_INVIS)
            duDebugDrawNavMeshWithClosedList(&dd, *m_navMesh, *m_navQuery, m_navMeshDrawFlags);
        if (m_drawMode == DRAWMODE_NAVMESH_BVTREE)
            duDebugDrawNavMeshBVTree(&dd, *m_navMesh);
        if (m_drawMode == DRAWMODE_NAVMESH_PORTALS)
            duDebugDrawNavMeshPortals(&dd, *m_navMesh);
        if (m_drawMode == DRAWMODE_NAVMESH_NODES)
            duDebugDrawNavMeshNodes(&dd, *m_navQuery);
    }
    
    
    glDepthMask(GL_TRUE);
    
    if (m_chf && m_drawMode == DRAWMODE_COMPACT)
        duDebugDrawCompactHeightfieldSolid(&dd, *m_chf);
    
    if (m_chf && m_drawMode == DRAWMODE_COMPACT_DISTANCE)
        duDebugDrawCompactHeightfieldDistance(&dd, *m_chf);
    if (m_chf && m_drawMode == DRAWMODE_COMPACT_REGIONS)
        duDebugDrawCompactHeightfieldRegions(&dd, *m_chf);
    if (m_solid && m_drawMode == DRAWMODE_VOXELS)
    {
        glEnable(GL_FOG);
        duDebugDrawHeightfieldSolid(&dd, *m_solid);
        glDisable(GL_FOG);
    }
    if (m_solid && m_drawMode == DRAWMODE_VOXELS_WALKABLE)
    {
        glEnable(GL_FOG);
        duDebugDrawHeightfieldWalkable(&dd, *m_solid);
        glDisable(GL_FOG);
    }
    if (m_cset && m_drawMode == DRAWMODE_RAW_CONTOURS)
    {
        glDepthMask(GL_FALSE);
        duDebugDrawRawContours(&dd, *m_cset);
        glDepthMask(GL_TRUE);
    }
    if (m_cset && m_drawMode == DRAWMODE_BOTH_CONTOURS)
    {
        glDepthMask(GL_FALSE);
        duDebugDrawRawContours(&dd, *m_cset, 0.5f);
        duDebugDrawContours(&dd, *m_cset);
        glDepthMask(GL_TRUE);
    }
    if (m_cset && m_drawMode == DRAWMODE_CONTOURS)
    {
        glDepthMask(GL_FALSE);
        duDebugDrawContours(&dd, *m_cset);
        glDepthMask(GL_TRUE);
    }
    if (m_chf && m_cset && m_drawMode == DRAWMODE_REGION_CONNECTIONS)
    {
        duDebugDrawCompactHeightfieldRegions(&dd, *m_chf);
        
        glDepthMask(GL_FALSE);
        duDebugDrawRegionConnections(&dd, *m_cset);
        glDepthMask(GL_TRUE);
    }
    if (m_pmesh && m_drawMode == DRAWMODE_POLYMESH)
    {
        glDepthMask(GL_FALSE);
        duDebugDrawPolyMesh(&dd, *m_pmesh);
        glDepthMask(GL_TRUE);
    }
    if (m_dmesh && m_drawMode == DRAWMODE_POLYMESH_DETAIL)
    {
        glDepthMask(GL_FALSE);
        duDebugDrawPolyMeshDetail(&dd, *m_dmesh);
        glDepthMask(GL_TRUE);
    }
    
    m_geom->drawConvexVolumes(&dd);
    
    if (m_tool)
        m_tool->handleRender();
    
    glDepthMask(GL_TRUE);
}

void Sample_TileMesh::handleRenderOverlay(double* proj, double* model, int* view)
{
    GLdouble x, y, z;
    
    // Draw start and end point labels
    if (m_tileBuildTime > 0.0f && gluProject((GLdouble)(m_tileBmin[0]+m_tileBmax[0])/2, (GLdouble)(m_tileBmin[1]+m_tileBmax[1])/2, (GLdouble)(m_tileBmin[2]+m_tileBmax[2])/2,
                                             model, proj, view, &x, &y, &z))
    {
        char text[32];
        snprintf(text,32,"%.3fms / %dTris / %.1fkB", m_tileBuildTime, m_tileTriCount, m_tileMemUsage);
        imguiDrawText((int)x, (int)y-25, IMGUI_ALIGN_CENTER, text, imguiRGBA(0,0,0,220));
    }
    
    if (m_tool)
        m_tool->handleRenderOverlay(proj, model, view);
}

void Sample_TileMesh::handleMeshChanged(class InputGeom* geom)
{
    Sample::handleMeshChanged(geom);

    cleanup();

    dtFreeNavMesh(m_navMesh);
    m_navMesh = 0;

    if (m_tool)
    {
        m_tool->reset();
        m_tool->init(this);
    }
}

bool Sample_TileMesh::handleBuild()
{
    if (!m_geom || !m_geom->getMesh())
    {
        m_ctx->log(RC_LOG_ERROR, "buildTiledNavigation: No vertices and triangles.");
        return false;
    }
    
    dtFreeNavMesh(m_navMesh);
    
    m_navMesh = dtAllocNavMesh();
    if (!m_navMesh)
    {
        m_ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Could not allocate navmesh.");
        return false;
    }

    dtNavMeshParams params;
    rcVcopy(params.orig, m_geom->getMeshBoundsMin());
    params.tileWidth = m_tileSize*m_cellSize;
    params.tileHeight = m_tileSize*m_cellSize;
    params.maxTiles = m_maxTiles;
    params.maxPolys = m_maxPolysPerTile;
    
    dtStatus status;
    
    status = m_navMesh->init(&params);
    if (dtStatusFailed(status))
    {
        m_ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Could not init navmesh.");
        return false;
    }
    
    status = m_navQuery->init(m_navMesh, 2048);
    if (dtStatusFailed(status))
    {
        m_ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Could not init Detour navmesh query");
        return false;
    }
    
    if (m_buildAll)
        buildAllTiles();
    
    if (m_tool)
        m_tool->init(this);

    return true;
}

void Sample_TileMesh::buildTile(const float* pos)
{
    if (!m_geom) return;
    if (!m_navMesh) return;
        
    const float* bmin = m_geom->getMeshBoundsMin();
    const float* bmax = m_geom->getMeshBoundsMax();
    
    const float ts = m_tileSize*m_cellSize;
    const int tx = (int)((pos[0] - bmin[0]) / ts);
    const int ty = (int)((pos[2] - bmin[2]) / ts);
    
    m_tileBmin[0] = bmin[0] + tx*ts;
    m_tileBmin[1] = bmin[1];
    m_tileBmin[2] = bmin[2] + ty*ts;
    
    m_tileBmax[0] = bmin[0] + (tx+1)*ts;
    m_tileBmax[1] = bmax[1];
    m_tileBmax[2] = bmin[2] + (ty+1)*ts;
    
    m_tileCol = duRGBA(77,204,0,255);
    
    m_ctx->resetLog();
    
    int dataSize = 0;
    unsigned char* data = buildTileMesh(tx, ty, m_tileBmin, m_tileBmax, dataSize);
    
    if (data)
    {
        // Remove any previous data (navmesh owns and deletes the data).
        m_navMesh->removeTile(m_navMesh->getTileRefAt(tx,ty),0,0);
        
        // Let the navmesh own the data.
        dtStatus status = m_navMesh->addTile(data,dataSize,DT_TILE_FREE_DATA,0,0);
        if (dtStatusFailed(status))
            dtFree(data);
    }
    
    m_ctx->dumpLog("Build Tile (%d,%d):", tx,ty);
}

void Sample_TileMesh::getTilePos(const float* pos, int& tx, int& ty)
{
    if (!m_geom) return;
    
    const float* bmin = m_geom->getMeshBoundsMin();
    
    const float ts = m_tileSize*m_cellSize;
    tx = (int)((pos[0] - bmin[0]) / ts);
    ty = (int)((pos[2] - bmin[2]) / ts);
}

void Sample_TileMesh::removeTile(const float* pos)
{
    if (!m_geom) return;
    if (!m_navMesh) return;
    
    const float* bmin = m_geom->getMeshBoundsMin();
    const float* bmax = m_geom->getMeshBoundsMax();

    const float ts = m_tileSize*m_cellSize;
    const int tx = (int)((pos[0] - bmin[0]) / ts);
    const int ty = (int)((pos[2] - bmin[2]) / ts);
    
    m_tileBmin[0] = bmin[0] + tx*ts;
    m_tileBmin[1] = bmin[1];
    m_tileBmin[2] = bmin[2] + ty*ts;
    
    m_tileBmax[0] = bmin[0] + (tx+1)*ts;
    m_tileBmax[1] = bmax[1];
    m_tileBmax[2] = bmin[2] + (ty+1)*ts;
    
    m_tileCol = duRGBA(204,25,0,255);
    
    m_navMesh->removeTile(m_navMesh->getTileRefAt(tx,ty),0,0);
}

void Sample_TileMesh::buildAllTiles()
{
    if (!m_geom) return;
    if (!m_navMesh) return;
    
    const float* bmin = m_geom->getMeshBoundsMin();
    const float* bmax = m_geom->getMeshBoundsMax();
    int gw = 0, gh = 0;
    rcCalcGridSize(bmin, bmax, m_cellSize, &gw, &gh);
    const int ts = (int)m_tileSize;
    const int tw = (gw + ts-1) / ts;
    const int th = (gh + ts-1) / ts;
    const float tcs = m_tileSize*m_cellSize;


    // Start the build process.
    m_ctx->startTimer(RC_TIMER_TEMP);

    for (int y = 0; y < th; ++y)
    {
        for (int x = 0; x < tw; ++x)
        {
            m_tileBmin[0] = bmin[0] + x*tcs;
            m_tileBmin[1] = bmin[1];
            m_tileBmin[2] = bmin[2] + y*tcs;
            
            m_tileBmax[0] = bmin[0] + (x+1)*tcs;
            m_tileBmax[1] = bmax[1];
            m_tileBmax[2] = bmin[2] + (y+1)*tcs;
            
            int dataSize = 0;
            unsigned char* data = buildTileMesh(x, y, m_tileBmin, m_tileBmax, dataSize);
            if (data)
            {
                // Remove any previous data (navmesh owns and deletes the data).
                m_navMesh->removeTile(m_navMesh->getTileRefAt(x,y),0,0);
                // Let the navmesh own the data.
                dtStatus status = m_navMesh->addTile(data,dataSize,DT_TILE_FREE_DATA,0,0);
                if (dtStatusFailed(status))
                    dtFree(data);
            }
        }
    }
    
    // Start the build process.    
    m_ctx->startTimer(RC_TIMER_TEMP);

    m_totalBuildTimeMs = m_ctx->getAccumulatedTime(RC_TIMER_TEMP)/1000.0f;
}

void Sample_TileMesh::removeAllTiles()
{
    const float* bmin = m_geom->getMeshBoundsMin();
    const float* bmax = m_geom->getMeshBoundsMax();
    int gw = 0, gh = 0;
    rcCalcGridSize(bmin, bmax, m_cellSize, &gw, &gh);
    const int ts = (int)m_tileSize;
    const int tw = (gw + ts-1) / ts;
    const int th = (gh + ts-1) / ts;
    
    for (int y = 0; y < th; ++y)
        for (int x = 0; x < tw; ++x)
            m_navMesh->removeTile(m_navMesh->getTileRefAt(x,y),0,0);
}

unsigned char* Sample_TileMesh::buildTileMesh(const int tx, const int ty, const float* bmin, const float* bmax, int& dataSize)
{
    if (!m_geom || !m_geom->getMesh() || !m_geom->getChunkyMesh())
    {
        m_ctx->log(RC_LOG_ERROR, "buildNavigation: Input mesh is not specified.");
        return 0;
    }
    
    m_tileMemUsage = 0;
    m_tileBuildTime = 0;
    
    cleanup();
    
    const float* verts = m_geom->getMesh()->getVerts();
    const int nverts = m_geom->getMesh()->getVertCount();
    const int ntris = m_geom->getMesh()->getTriCount();
    const rcChunkyTriMesh* chunkyMesh = m_geom->getChunkyMesh();
        
    // Init build configuration from GUI
    memset(&m_cfg, 0, sizeof(m_cfg));
    m_cfg.cs = m_cellSize;
    m_cfg.ch = m_cellHeight;
    m_cfg.walkableSlopeAngle = m_agentMaxSlope;
    m_cfg.walkableHeight = (int)ceilf(m_agentHeight / m_cfg.ch);
    m_cfg.walkableClimb = (int)floorf(m_agentMaxClimb / m_cfg.ch);
    m_cfg.walkableRadius = (int)ceilf(m_agentRadius / m_cfg.cs);
    m_cfg.maxEdgeLen = (int)(m_edgeMaxLen / m_cellSize);
    m_cfg.maxSimplificationError = m_edgeMaxError;
    m_cfg.minRegionArea = (int)rcSqr(m_regionMinSize);        // Note: area = size*size
    m_cfg.mergeRegionArea = (int)rcSqr(m_regionMergeSize);    // Note: area = size*size
    m_cfg.maxVertsPerPoly = (int)m_vertsPerPoly;
    m_cfg.tileSize = (int)m_tileSize;
    m_cfg.borderSize = m_cfg.walkableRadius + 3; // Reserve enough padding.
    m_cfg.width = m_cfg.tileSize + m_cfg.borderSize*2;
    m_cfg.height = m_cfg.tileSize + m_cfg.borderSize*2;
    m_cfg.detailSampleDist = m_detailSampleDist < 0.9f ? 0 : m_cellSize * m_detailSampleDist;
    m_cfg.detailSampleMaxError = m_cellHeight * m_detailSampleMaxError;
    
    rcVcopy(m_cfg.bmin, bmin);
    rcVcopy(m_cfg.bmax, bmax);
    m_cfg.bmin[0] -= m_cfg.borderSize*m_cfg.cs;
    m_cfg.bmin[2] -= m_cfg.borderSize*m_cfg.cs;
    m_cfg.bmax[0] += m_cfg.borderSize*m_cfg.cs;
    m_cfg.bmax[2] += m_cfg.borderSize*m_cfg.cs;
    
    // Reset build times gathering.
    m_ctx->resetTimers();
    
    // Start the build process.
    m_ctx->startTimer(RC_TIMER_TOTAL);
    
    m_ctx->log(RC_LOG_PROGRESS, "Building navigation:");
    m_ctx->log(RC_LOG_PROGRESS, " - %d x %d cells", m_cfg.width, m_cfg.height);
    m_ctx->log(RC_LOG_PROGRESS, " - %.1fK verts, %.1fK tris", nverts/1000.0f, ntris/1000.0f);
    
    // Allocate voxel heightfield where we rasterize our input data to.
    m_solid = rcAllocHeightfield();
    if (!m_solid)
    {
        m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'solid'.");
        return 0;
    }
    if (!rcCreateHeightfield(m_ctx, *m_solid, m_cfg.width, m_cfg.height, m_cfg.bmin, m_cfg.bmax, m_cfg.cs, m_cfg.ch))
    {
        m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create solid heightfield.");
        return 0;
    }
    
    // Allocate array that can hold triangle flags.
    // If you have multiple meshes you need to process, allocate
    // and array which can hold the max number of triangles you need to process.
    m_triareas = new unsigned char[chunkyMesh->maxTrisPerChunk];
    if (!m_triareas)
    {
        m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'm_triareas' (%d).", chunkyMesh->maxTrisPerChunk);
        return 0;
    }
    
    float tbmin[2], tbmax[2];
    tbmin[0] = m_cfg.bmin[0];
    tbmin[1] = m_cfg.bmin[2];
    tbmax[0] = m_cfg.bmax[0];
    tbmax[1] = m_cfg.bmax[2];
    int cid[512];// TODO: Make grow when returning too many items.
    const int ncid = rcGetChunksOverlappingRect(chunkyMesh, tbmin, tbmax, cid, 512);
    if (!ncid)
        return 0;
    
    m_tileTriCount = 0;
    
    for (int i = 0; i < ncid; ++i)
    {
        const rcChunkyTriMeshNode& node = chunkyMesh->nodes[cid[i]];
        const int* tris = &chunkyMesh->tris[node.i*3];
        const int ntris = node.n;
        
        m_tileTriCount += ntris;
        
        memset(m_triareas, 0, ntris*sizeof(unsigned char));
        rcMarkWalkableTriangles(m_ctx, m_cfg.walkableSlopeAngle,
                                verts, nverts, tris, ntris, m_triareas);
        
        rcRasterizeTriangles(m_ctx, verts, nverts, tris, m_triareas, ntris, *m_solid, m_cfg.walkableClimb);
    }
    
    if (!m_keepInterResults)
    {
        delete [] m_triareas;
        m_triareas = 0;
    }
    
    // Once all geometry is rasterized, we do initial pass of filtering to
    // remove unwanted overhangs caused by the conservative rasterization
    // as well as filter spans where the character cannot possibly stand.
    rcFilterLowHangingWalkableObstacles(m_ctx, m_cfg.walkableClimb, *m_solid);
    rcFilterLedgeSpans(m_ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid);
    rcFilterWalkableLowHeightSpans(m_ctx, m_cfg.walkableHeight, *m_solid);
    
    // Compact the heightfield so that it is faster to handle from now on.
    // This will result more cache coherent data as well as the neighbours
    // between walkable cells will be calculated.
    m_chf = rcAllocCompactHeightfield();
    if (!m_chf)
    {
        m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'chf'.");
        return 0;
    }
    if (!rcBuildCompactHeightfield(m_ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid, *m_chf))
    {
        m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build compact data.");
        return 0;
    }
    
    if (!m_keepInterResults)
    {
        rcFreeHeightField(m_solid);
        m_solid = 0;
    }

    // Erode the walkable area by agent radius.
    if (!rcErodeWalkableArea(m_ctx, m_cfg.walkableRadius, *m_chf))
    {
        m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not erode.");
        return false;
    }

    // (Optional) Mark areas.
    const ConvexVolume* vols = m_geom->getConvexVolumes();
    for (int i  = 0; i < m_geom->getConvexVolumeCount(); ++i)
        rcMarkConvexPolyArea(m_ctx, vols[i].verts, vols[i].nverts, vols[i].hmin, vols[i].hmax, (unsigned char)vols[i].area, *m_chf);
    
    // Prepare for region partitioning, by calculating distance field along the walkable surface.
    if (!rcBuildDistanceField(m_ctx, *m_chf))
    {
        m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build distance field.");
        return 0;
    }
    
    // Partition the walkable surface into simple regions without holes.
    if (!rcBuildRegions(m_ctx, *m_chf, m_cfg.borderSize, m_cfg.minRegionArea, m_cfg.mergeRegionArea))
    {
        m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build regions.");
        return 0;
    }
 
    // Create contours.
    m_cset = rcAllocContourSet();
    if (!m_cset)
    {
        m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'cset'.");
        return 0;
    }
    if (!rcBuildContours(m_ctx, *m_chf, m_cfg.maxSimplificationError, m_cfg.maxEdgeLen, *m_cset))
    {
        m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create contours.");
        return 0;
    }
    
    if (m_cset->nconts == 0)
    {
        return 0;
    }
    
    // Build polygon navmesh from the contours.
    m_pmesh = rcAllocPolyMesh();
    if (!m_pmesh)
    {
        m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmesh'.");
        return 0;
    }
    if (!rcBuildPolyMesh(m_ctx, *m_cset, m_cfg.maxVertsPerPoly, *m_pmesh))
    {
        m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not triangulate contours.");
        return 0;
    }
    
    // Build detail mesh.
    m_dmesh = rcAllocPolyMeshDetail();
    if (!m_dmesh)
    {
        m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'dmesh'.");
        return 0;
    }
    
    if (!rcBuildPolyMeshDetail(m_ctx, *m_pmesh, *m_chf,
                               m_cfg.detailSampleDist, m_cfg.detailSampleMaxError,
                               *m_dmesh))
    {
        m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could build polymesh detail.");
        return 0;
    }
    
    if (!m_keepInterResults)
    {
        rcFreeCompactHeightfield(m_chf);
        m_chf = 0;
        rcFreeContourSet(m_cset);
        m_cset = 0;
    }
    
    unsigned char* navData = 0;
    int navDataSize = 0;
    if (m_cfg.maxVertsPerPoly <= DT_VERTS_PER_POLYGON)
    {
        // Remove padding from the polymesh data. TODO: Remove this odditity.
        for (int i = 0; i < m_pmesh->nverts; ++i)
        {
            unsigned short* v = &m_pmesh->verts[i*3];
            v[0] -= (unsigned short)m_cfg.borderSize;
            v[2] -= (unsigned short)m_cfg.borderSize;
        }
        
        if (m_pmesh->nverts >= 0xffff)
        {
            // The vertex indices are ushorts, and cannot point to more than 0xffff vertices.
            m_ctx->log(RC_LOG_ERROR, "Too many vertices per tile %d (max: %d).", m_pmesh->nverts, 0xffff);
            return false;
        }
        
        // Update poly flags from areas.
        for (int i = 0; i < m_pmesh->npolys; ++i)
        {
            if (m_pmesh->areas[i] == RC_WALKABLE_AREA)
                m_pmesh->areas[i] = SAMPLE_POLYAREA_GROUND;
            
            if (m_pmesh->areas[i] == SAMPLE_POLYAREA_GROUND ||
                m_pmesh->areas[i] == SAMPLE_POLYAREA_GRASS ||
                m_pmesh->areas[i] == SAMPLE_POLYAREA_ROAD)
            {
                m_pmesh->flags[i] = SAMPLE_POLYFLAGS_WALK;
            }
            else if (m_pmesh->areas[i] == SAMPLE_POLYAREA_WATER)
            {
                m_pmesh->flags[i] = SAMPLE_POLYFLAGS_SWIM;
            }
            else if (m_pmesh->areas[i] == SAMPLE_POLYAREA_DOOR)
            {
                m_pmesh->flags[i] = SAMPLE_POLYFLAGS_WALK | SAMPLE_POLYFLAGS_DOOR;
            }
        }
        
        dtNavMeshCreateParams params;
        memset(&params, 0, sizeof(params));
        params.verts = m_pmesh->verts;
        params.vertCount = m_pmesh->nverts;
        params.polys = m_pmesh->polys;
        params.polyAreas = m_pmesh->areas;
        params.polyFlags = m_pmesh->flags;
        params.polyCount = m_pmesh->npolys;
        params.nvp = m_pmesh->nvp;
        params.detailMeshes = m_dmesh->meshes;
        params.detailVerts = m_dmesh->verts;
        params.detailVertsCount = m_dmesh->nverts;
        params.detailTris = m_dmesh->tris;
        params.detailTriCount = m_dmesh->ntris;
        params.offMeshConVerts = m_geom->getOffMeshConnectionVerts();
        params.offMeshConRad = m_geom->getOffMeshConnectionRads();
        params.offMeshConDir = m_geom->getOffMeshConnectionDirs();
        params.offMeshConAreas = m_geom->getOffMeshConnectionAreas();
        params.offMeshConFlags = m_geom->getOffMeshConnectionFlags();
        params.offMeshConUserID = m_geom->getOffMeshConnectionId();
        params.offMeshConCount = m_geom->getOffMeshConnectionCount();
        params.walkableHeight = m_agentHeight;
        params.walkableRadius = m_agentRadius;
        params.walkableClimb = m_agentMaxClimb;
        params.tileX = tx;
        params.tileY = ty;
        rcVcopy(params.bmin, bmin);
        rcVcopy(params.bmax, bmax);
        params.cs = m_cfg.cs;
        params.ch = m_cfg.ch;
        params.tileSize = m_cfg.tileSize;
        
        if (!dtCreateNavMeshData(&params, &navData, &navDataSize))
        {
            m_ctx->log(RC_LOG_ERROR, "Could not build Detour navmesh.");
            return 0;
        }
        
        // Restore padding so that the debug visualization is correct.
        for (int i = 0; i < m_pmesh->nverts; ++i)
        {
            unsigned short* v = &m_pmesh->verts[i*3];
            v[0] += (unsigned short)m_cfg.borderSize;
            v[2] += (unsigned short)m_cfg.borderSize;
        }
        
    }
    m_tileMemUsage = navDataSize/1024.0f;
    
    m_ctx->stopTimer(RC_TIMER_TOTAL);
    
    // Show performance stats.
    duLogBuildTimes(*m_ctx, m_ctx->getAccumulatedTime(RC_TIMER_TOTAL));
    m_ctx->log(RC_LOG_PROGRESS, ">> Polymesh: %d vertices  %d polygons", m_pmesh->nverts, m_pmesh->npolys);
    
    m_tileBuildTime = m_ctx->getAccumulatedTime(RC_TIMER_TOTAL)/1000.0f;

    dataSize = navDataSize;
    return navData;
}
